#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/Header.h>
#include "lane_node/lane_line_single.h"
#include "lane_node/lane_line_list.h"
#include <cv_bridge/cv_bridge.h>
#include "helper.h"
#include "lane_class.h"

//这行用于声明节点使用的消息文件所在的命名空间。集成编译时删掉下面这行 
using namespace lane_node;

class LaneDetector{
    private:
        image_transport::Subscriber sub;
        ros::Publisher pub;
        lane_class lane;
        lane_line_single left_line;
        lane_line_single right_line;
        lane_line_list line_list;
    public:
        LaneDetector(){
            ros::NodeHandle nh;
            image_transport::ImageTransport it(nh);
            sub = it.subscribe("cam_cap_1", 1, &LaneDetector::callback, this);
            pub = nh.advertise<lane_line_list>("lane_line", 100);
            left_line.degree = 2;
            left_line.pos = 1;
            right_line.degree = 2;
            right_line.pos = 2;
        }
        void callback(const sensor_msgs::ImageConstPtr& msg){
            
            if(lane.is_initialized && lane.bad_fit_count < 5)
                lane.update(cv_bridge::toCvShare(msg, "bgr8")->image);
            else
                lane.init(cv_bridge::toCvShare(msg, "bgr8")->image);
            
	    line_list.lines.clear();
	    left_line.coefficients.clear();
	    right_line.coefficients.clear();

            if(lane.is_initialized && lane.bad_fit_count < 5){
                for(int i=0; i<3; ++i){
                    left_line.coefficients.push_back(lane.left_fit[i]);
                    right_line.coefficients.push_back(lane.right_fit[i]);
                }
                line_list.lines.push_back(left_line);
                line_list.lines.push_back(right_line);
                line_list.is_lost = false;
            }
            else{
                line_list.is_lost = true;
            }
            line_list.header.stamp = msg -> header.stamp;
            pub.publish(line_list);
        }
};

int main(int argc, char **argv)  
{  
  ros::init(argc, argv, "lane_node");  
  LaneDetector lane_detector;
  ros::spin();  
  return 0;
}

